#! /usr/bin/env python
#coding=utf-8
import rospy
import actionlib
from geometry_msgs.msg import PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from megatron.msg import wr
rospy.init_node('subrviz')
a=[0]*8
i=1
def liscb(mm):     
    global i
    with open('/home/xbot/catkin_ws/src/megatron/src/include/data.txt','r') as f:
        c = f.read()
    if c=='0':
        a[1] = mm.pose.position.x
        a[2] = mm.pose.position.y
        a[3] = mm.pose.position.z
        a[4] = mm.pose.orientation.x
        a[5] = mm.pose.orientation.y
        a[6] = mm.pose.orientation.z
        a[7] = mm.pose.orientation.w
        rospy.set_param('point'+str(i),a)
        print('\nListener: We\'ve set %d goals!\n' % i)
        i+=1
        

def listener():
    rospy.Subscriber('move_base_simple/goal', PoseStamped, liscb)
    rospy.spin()

if __name__ == '__main__':
    print('\nListener is ready, choose our goals!\n')
    listener()
